#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <chrono>
#include <memory>

using namespace std::chrono_literals;

class SquareTestNode : public rclcpp::Node
{
public:
    SquareTestNode() : Node("square_test_node"), state_(0)
    {
        // 创建速度发布器
        cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 10);
        
        // 创建定时器，100ms执行一次
        timer_ = this->create_wall_timer(
            100ms, std::bind(&SquareTestNode::timer_callback, this));
        
        // 初始化参数
        side_length_ = 0.5;  // 正方形边长1米
        linear_speed_ = 0.2; // 线速度0.2m/s
        angular_speed_ = 0.2; // 角速度0.2rad/s
        
        start_time_ = this->now();
        RCLCPP_INFO(this->get_logger(), "Square test node started");
    }

    ~SquareTestNode()
    {
        auto msg = geometry_msgs::msg::Twist();
        msg.linear.x = 0;
        msg.linear.y = 0;
        msg.angular.z = 0;
        
        cmd_vel_pub_->publish(msg);
        RCLCPP_INFO(this->get_logger(), "Square test node stopped");
    }

private:
    void timer_callback()
    {
        auto msg = geometry_msgs::msg::Twist();
        auto current_time = this->now();
        double time_elapsed = (current_time - start_time_).seconds();
        
        // 状态机控制
        switch(state_) {
            case 0: // 前进
                msg.linear.x = linear_speed_;
                if (time_elapsed >= side_length_ / linear_speed_) {
                    state_ = 1;
                    start_time_ = current_time;
                    RCLCPP_INFO(this->get_logger(), "Turning left");
                }
                break;
                
            case 1: // 左转
                msg.angular.z = angular_speed_;
                if (time_elapsed >= M_PI_2 / angular_speed_) {
                    state_ = (state_ + 1) % 8;
                    start_time_ = current_time;
                    RCLCPP_INFO(this->get_logger(), "Moving forward");
                }
                break;
                
            case 2: // 前进
                msg.linear.x = linear_speed_;
                if (time_elapsed >= side_length_ / linear_speed_) {
                    state_ = 3;
                    start_time_ = current_time;
                    RCLCPP_INFO(this->get_logger(), "Turning left");
                }
                break;
                
            case 3: // 左转
                msg.angular.z = angular_speed_;
                if (time_elapsed >= M_PI_2 / angular_speed_) {
                    state_ = (state_ + 1) % 8;
                    start_time_ = current_time;
                    RCLCPP_INFO(this->get_logger(), "Moving forward");
                }
                break;
                
            case 4: // 前进
                msg.linear.x = linear_speed_;
                if (time_elapsed >= side_length_ / linear_speed_) {
                    state_ = 5;
                    start_time_ = current_time;
                    RCLCPP_INFO(this->get_logger(), "Turning left");
                }
                break;
                
            case 5: // 左转
                msg.angular.z = angular_speed_;
                if (time_elapsed >= M_PI_2 / angular_speed_) {
                    state_ = (state_ + 1) % 8;
                    start_time_ = current_time;
                    RCLCPP_INFO(this->get_logger(), "Moving forward");
                }
                break;
                
            case 6: // 前进
                msg.linear.x = linear_speed_;
                if (time_elapsed >= side_length_ / linear_speed_) {
                    state_ = 7;
                    start_time_ = current_time;
                    RCLCPP_INFO(this->get_logger(), "Final turn");
                }
                break;
                
            case 7: // 最后一次左转
                msg.angular.z = angular_speed_;
                if (time_elapsed >= M_PI_2 / angular_speed_) {
                    state_ = 0;
                    start_time_ = current_time;
                    RCLCPP_INFO(this->get_logger(), "Square completed!");
                }
                break;
        }
        
        cmd_vel_pub_->publish(msg);
    }

    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Time start_time_;
    
    int state_;  // 状态机状态
    double side_length_;  // 正方形边长
    double linear_speed_;  // 线速度
    double angular_speed_;  // 角速度
};

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<SquareTestNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
